#include "SerialControl.h"
#include <vector>
#include <string>
#include <iostream>
#include <sstream>

// 重载 operator<< 以支持 std::vector<double> 的输出
std::ostream& operator<<(std::ostream& os, const std::vector<double>& vec) {
    os << "[";
    for (size_t i = 0; i < vec.size(); ++i) {
        os << vec[i];
        if (i != vec.size() - 1) {
            os << ", ";
        }
    }
    os << "]";
    return os;
}

int main(int argc, char** argv) 
{
    // **************************"写入示例代码"**************************
    std::vector<double> rads = {0.5, 0.5, 0.5, 0.5, 0.5, 0.5};

    SerialControl ser("/dev/ttyUSB0", 115200, 1000); // 1000ms

    // ser.sendCommand(rads, "arm");

    // ser.sendCommand("#026P1300T0800!");


    // **************************"读取示例代码"**************************
    // SerialControl ser("/dev/ttyUSB0", 115200, 1000);

    // std::vector<double> data = ser.getRadsData("arm");

    // std::cout << "data : " << data << std::endl;
    while (1)
    {   std::cout << "Press 'r' to read data or 'w' to write data or any other key to exit: ";
        if (std::cin.get() == 'r')
        {
            std::vector<double> data = ser.getRadsData("arm");
        }
        else if (std::cin.get() == 'w')
        {
            std::cout << "Enter radians values (comma-separated, e.g., 0.5,0.5,0.5,0.5,0.5,0.5): ";
            std::string input;
            std::cin.ignore(); // Clear the input buffer
            std::getline(std::cin, input); // Read the entire line

            // Parse the input into a vector of doubles
            std::vector<double> rads;
            std::stringstream ss(input);
            std::string token;
            while (std::getline(ss, token, ',')) {
                try {
                    rads.push_back(std::stod(token));
                } catch (const std::invalid_argument& e) {
                    std::cerr << "Invalid input: " << token << " is not a valid number." << std::endl;
                    rads.clear();
                    break;
                }
            }

            if (!rads.empty()) {
                ser.sendCommand(rads, "arm");
                std::cout << "Command sent: ";
                for (double r : rads) {
                    std::cout << r << " ";
                }
                std::cout << std::endl;
            } else {
                std::cout << "No valid radians values were entered." << std::endl;
            }
        }
        else if (std::cin.get() == 'q')
        {
            std::cout << "exit" << std::endl;
            ser.closeSerial();
            break;
        }
        else
        {
            /* code */
            std::cout << "invalid letter" << std::endl;
            // ser.closeSerial();
        }
        
    }

    return 0;

}